#include "dji_motor.h"

#include "bsp_dwt.h"
#include "bsp_log.h"
#include "general_def.h"

// TODO: FDCAN should be downward compatible with CAN, interface & init function should be modified

static uint8_t idx = 0;  // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
static DJIMotorInstance* dji_motor_instance[DJI_MOTOR_CNT] = {NULL};  // 会在control任务中遍历该指针数组进行pid计算

/**
 * @brief 由于DJI电机发送以四个一组的形式进行,故对其进行特殊处理,用5个(2fdcan*3group)can_instance专门负责发送
 *        该变量将在 DJIMotorControl() 中使用,分组在 MotorSenderGrouping()中进行
 *
 * @note  因为只用于发送,所以不需要在bsp_can中注册
 *
 * C609(m2006)/C620(m3508):0x1ff,0x200;
 * GM6019:0x1ff,0x2ff
 * 反馈(rx_id): GM6019: 0x204+id ; C610/C620: 0x200+id
 * fdcan0: [0]:0x1FF,[1]:0x200,[2]:0x2FF
 * fdcan1: [3]:0x1FF,[4]:0x200,[5]:0x2FF
 */
#ifdef STM32F407xx
static CANInstance sender_assignment[6] = {
    [0] = {.can_handle = &hcan1,
           .txconf.StdId = 0x1ff,
           .txconf.IDE = CAN_ID_STD,
           .txconf.RTR = CAN_RTR_DATA,
           .txconf.DLC = 0x08,
           .tx_buff = {0}},
    [1] = {.can_handle = &hcan1,
           .txconf.StdId = 0x200,
           .txconf.IDE = CAN_ID_STD,
           .txconf.RTR = CAN_RTR_DATA,
           .txconf.DLC = 0x08,
           .tx_buff = {0}},
    [2] = {.can_handle = &hcan1,
           .txconf.StdId = 0x2ff,
           .txconf.IDE = CAN_ID_STD,
           .txconf.RTR = CAN_RTR_DATA,
           .txconf.DLC = 0x08,
           .tx_buff = {0}},
    [3] = {.can_handle = &hcan2,
           .txconf.StdId = 0x1ff,
           .txconf.IDE = CAN_ID_STD,
           .txconf.RTR = CAN_RTR_DATA,
           .txconf.DLC = 0x08,
           .tx_buff = {0}},
    [4] = {.can_handle = &hcan2,
           .txconf.StdId = 0x200,
           .txconf.IDE = CAN_ID_STD,
           .txconf.RTR = CAN_RTR_DATA,
           .txconf.DLC = 0x08,
           .tx_buff = {0}},
    [5] = {.can_handle = &hcan2,
           .txconf.StdId = 0x2ff,
           .txconf.IDE = CAN_ID_STD,
           .txconf.RTR = CAN_RTR_DATA,
           .txconf.DLC = 0x08,
           .tx_buff = {0}},
};
#elifdef STM32H723xx
#define FDCAN_INSTANCE_INIT(fdcan_handle, tx_id)                               \
  {.can_handle = (fdcan_handle),                                               \
   .txconf = (FDCAN_TxHeaderTypeDef){.Identifier = (tx_id),                    \
                                     .IdType = FDCAN_STANDARD_ID,              \
                                     .TxFrameType = FDCAN_DATA_FRAME,          \
                                     .DataLength = FDCAN_DLC_BYTES_8,          \
                                     .ErrorStateIndicator = FDCAN_ESI_ACTIVE,  \
                                     .BitRateSwitch = FDCAN_BRS_OFF,           \
                                     .FDFormat = FDCAN_CLASSIC_CAN,            \
                                     .TxEventFifoControl = FDCAN_NO_TX_EVENTS, \
                                     .MessageMarker = 0},                      \
   .tx_buff = {0}}

static CANInstance sender_assignment[9] = {
    [0] = FDCAN_INSTANCE_INIT(&hfdcan1, 0x1FF), [1] = FDCAN_INSTANCE_INIT(&hfdcan1, 0x200),
    [2] = FDCAN_INSTANCE_INIT(&hfdcan1, 0x2FF), [3] = FDCAN_INSTANCE_INIT(&hfdcan2, 0x1FF),
    [4] = FDCAN_INSTANCE_INIT(&hfdcan2, 0x200), [5] = FDCAN_INSTANCE_INIT(&hfdcan2, 0x2FF),
    [6] = FDCAN_INSTANCE_INIT(&hfdcan3, 0x1FF), [7] = FDCAN_INSTANCE_INIT(&hfdcan3, 0x200),
    [8] = FDCAN_INSTANCE_INIT(&hfdcan3, 0x2FF),
};

#endif

/**
 * @brief 9个用于确认是否有电机注册到sender_assignment中的标志位,防止发送空帧,此变量将在DJIMotorControl()使用
 *        flag的初始化在 MotorSenderGrouping()中进行
 */
static uint8_t sender_enable_flag[9] = {0};

/**
 * @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
 *        并对电机进行分组以便处理多电机控制命令
 *
 * @param motor 电机实例指针
 * @param config 电机CAN初始化配置
 */
/**
 * @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
 *        并对电机进行分组以便处理多电机控制命令
 */

#ifdef STM32F407xx
static void MotorSenderGrouping(DJIMotorInstance* motor, CAN_Init_Config_s* config) {
  uint8_t motor_id = config->tx_id - 1;  // 下标从零开始,先减一方便赋值
  uint8_t motor_send_num;
  uint8_t motor_grouping;

  switch (motor->motor_type) {
    case M2006:
    case M3508:
      if (motor_id < 4)  // 根据ID分组
      {
        motor_send_num = motor_id;
        motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
      } else {
        motor_send_num = motor_id - 4;
        motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
      }

      // 计算接收id并设置分组发送id
      config->rx_id = 0x200 + motor_id + 1;    // 把ID+1,进行分组设置
      sender_enable_flag[motor_grouping] = 1;  // 设置发送标志位,防止发送空帧
      motor->message_num = motor_send_num;
      motor->sender_group = motor_grouping;

      // 检查是否发生id冲突
      for (size_t i = 0; i < idx; ++i) {
        if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
            dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
          LOGERROR(
              "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
          uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
          while (1)  // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
            LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
        }
      }
      break;

    case GM6020:
      if (motor_id < 4) {
        motor_send_num = motor_id;
        motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
      } else {
        motor_send_num = motor_id - 4;
        motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
      }

      config->rx_id = 0x204 + motor_id + 1;  // 把ID+1,进行分组设置
      sender_enable_flag[motor_grouping] =
          1;  // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
      motor->message_num = motor_send_num;
      motor->sender_group = motor_grouping;

      for (size_t i = 0; i < idx; ++i) {
        if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
            dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
          LOGERROR(
              "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
          uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
          while (1)  // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
            LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
        }
      }
      break;

    default:  // other motors should not be registered here
      while (1)
        LOGERROR(
            "[dji_motor]You must not register other motors using the API of DJI motor.");  // 其他电机不应该在这里注册
  }
}
#elifdef STM32H723xx
static void MotorSenderGrouping(DJIMotorInstance* motor, CAN_Init_Config_s* config) {
  // 修改：参数类型
  uint8_t motor_id = config->tx_id - 1;  // 下标从零开始,先减一方便赋值
  uint8_t motor_send_num;
  uint8_t motor_grouping;

  switch (motor->motor_type) {
    case M2006:
    case M3508:
      if (motor_id < 4)  // 根据ID分组
      {
        motor_send_num = motor_id;
        motor_send_num = motor_id;
        motor_grouping = config->can_handle == &hcan1
                             ? 1
                             : (config->can_handle == &hcan2 ? 4 : 7);  // 修改：can_handle → fdcan_handle
      } else {
        motor_send_num = motor_id - 4;
        motor_grouping = config->can_handle == &hcan1 ? 0 : (config->can_handle == &hcan2 ? 3 : 6);
      }

      // 计算接收id并设置分组发送id
      config->rx_id = 0x200 + motor_id + 1;    // 把ID+1,进行分组设置
      sender_enable_flag[motor_grouping] = 1;  // 设置发送标志位,防止发送空帧
      motor->message_num = motor_send_num;
      motor->sender_group = motor_grouping;

      // 检查是否发生id冲突
      for (size_t i = 0; i < idx; ++i) {
        if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&  // 修改：变量名
            dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
          LOGERROR(
              "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
          uint16_t fdcan_bus =
              config->can_handle == &hfdcan1 ? 1 : (config->can_handle == &hfdcan2 ? 2 : 3);  // 修改：变量名
          while (1)  // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
            LOGERROR("[dji_motor] id [%d], fdcan_bus [%d]", config->rx_id, fdcan_bus);
        }
      }
      break;

    case GM6020:
      if (motor_id < 4) {
        motor_send_num = motor_id;
        motor_grouping = config->can_handle == &hcan1 ? 0 : (config->can_handle == &hcan2 ? 3 : 6);
      } else {
        motor_send_num = motor_id - 4;
        motor_grouping = config->can_handle == &hcan1 ? 2 : (config->can_handle == &hcan2 ? 5 : 8);
      }

      config->rx_id = 0x204 + motor_id + 1;  // 把ID+1,进行分组设置
      sender_enable_flag[motor_grouping] =
          1;  // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
      motor->message_num = motor_send_num;
      motor->sender_group = motor_grouping;

      for (size_t i = 0; i < idx; ++i) {
        if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
            dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
          LOGERROR(
              "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
          uint16_t fdcan_bus = config->can_handle == &hfdcan1 ? 1 : (config->can_handle == &hcan2 ? 2 : 3);
          while (1)  // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
            LOGERROR("[dji_motor] id [%d], fdcan_bus [%d]", config->rx_id, fdcan_bus);
        }
      }
      break;

    default:  // other motors should not be registered here
      while (1)
        LOGERROR(
            "[dji_motor]You must not register other motors using the API of DJI motor.");  // 其他电机不应该在这里注册
  }
}
#endif

/**
 * @todo  是否可以简化多圈角度的计算？
 * @brief 根据返回的can_instance对反馈报文进行解析
 *
 * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
 */
static void DecodeDJIMotor(CANInstance* _instance) {
  // 修改：参数类型
  // 这里对fdcan instance的id进行了强制转换,从而获得电机的instance实例地址
  uint8_t* rxbuff = _instance->rx_buff;
  DJIMotorInstance* motor = (DJIMotorInstance*)_instance->id;
  DJI_Motor_Measure_s* measure = &motor->measure;  // measure要多次使用,保存指针减小访存开销

  DaemonReload(motor->daemon);
  motor->dt = DWT_GetDeltaT(&motor->feed_cnt);

  // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
  measure->last_ecd = measure->ecd;
  measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
  measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
  measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
                       RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
  measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
                          CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
  measure->temperature = rxbuff[6];

  // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
  if (measure->ecd - measure->last_ecd > 4096)
    measure->total_round--;
  else if (measure->ecd - measure->last_ecd < -4096)
    measure->total_round++;
  measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
}

/**
 * @brief 电机丢失回调函数，当守护进程检测到电机失去连接时调用
 *
 * @param motor_ptr 电机实例指针
 */
#ifdef STM32H7
static void DJIMotorLostCallback(void* motor_ptr) {
  DJIMotorInstance* motor = (DJIMotorInstance*)motor_ptr;
  uint16_t can_bus = motor->motor_can_instance->can_handle == &hfdcan1
                         ? 1
                         : (motor->motor_can_instance->can_handle == &hfdcan2 ? 2 : 3);  // 修改：变量名
  LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
}
#elifdef STM32F407xx
static void DJIMotorLostCallback(void* motor_ptr) {
  DJIMotorInstance* motor = (DJIMotorInstance*)motor_ptr;
  uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
  LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
}
#endif

/**
 * @brief 初始化DJI电机并返回电机实例
 *
 * @param config 电机初始化配置结构体
 * @return DJIMotorInstance* 电机实例指针
 */
DJIMotorInstance* DJIMotorInit(Motor_Init_Config_s* config) {
  DJIMotorInstance* instance = (DJIMotorInstance*)malloc(sizeof(DJIMotorInstance));
  memset(instance, 0, sizeof(DJIMotorInstance));

  // motor basic setting 电机基本设置
  instance->motor_type = config->motor_type;                          // 6020 or 2006 or 3508
  instance->motor_settings = config->controller_setting_init_config;  // 正反转,闭环类型等

  // motor controller init 电机控制器初始化
  PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
  PIDInit(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
  PIDInit(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
  instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
  instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
  instance->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
  instance->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
  // 后续增加电机前馈控制器(速度和电流)

  // 电机分组,因为至多4个电机可以共用一帧FDCAN控制报文
  MotorSenderGrouping(instance, &config->can_init_config);  // 修改：参数名

  // 注册电机到FDCAN总线
  config->can_init_config.can_module_callback = DecodeDJIMotor;          // set callback  // 修改：变量名
  config->can_init_config.id = instance;                                 // set id,eq to address(it is identity)
  instance->motor_can_instance = CANRegister(&config->can_init_config);  // 修改：函数名和变量名

  // 注册守护线程
  Daemon_Init_Config_s daemon_config = {
      .callback = DJIMotorLostCallback,
      .owner_id = instance,
      .reload_count = 2,  // 20ms未收到数据则丢失
  };
  instance->daemon = DaemonRegister(&daemon_config);

  DJIMotorEnable(instance);
  dji_motor_instance[idx++] = instance;
  return instance;
}

/**
 * @brief 更改电机反馈源
 *
 * @param motor 电机实例指针
 * @param loop 闭环类型（角度环/速度环）
 * @param type 反馈源类型（电机自身反馈/外部反馈）
 */
void DJIMotorChangeFeed(DJIMotorInstance* motor, Closeloop_Type_e loop, Feedback_Source_e type) {
  if (loop == ANGLE_LOOP)
    motor->motor_settings.angle_feedback_source = type;
  else if (loop == SPEED_LOOP)
    motor->motor_settings.speed_feedback_source = type;
  else
    LOGERROR(
        "[dji_motor] loop type error, check memory access and func param");  // 检查是否传入了正确的LOOP类型,或发生了指针越界
}

/**
 * @brief 停止电机运行
 *
 * @param motor 电机实例指针
 */
void DJIMotorStop(DJIMotorInstance* motor) { motor->stop_flag = MOTOR_STOP; }

/**
 * @brief 启用电机
 *
 * @param motor 电机实例指针
 */
void DJIMotorEnable(DJIMotorInstance* motor) { motor->stop_flag = MOTOR_ENALBED; }

/**
 * @brief 修改电机外环控制类型
 *
 * @param motor 电机实例指针
 * @param outer_loop 外环控制类型（角度环/速度环/无）
 */
void DJIMotorOuterLoop(DJIMotorInstance* motor, Closeloop_Type_e outer_loop) {
  motor->motor_settings.outer_loop_type = outer_loop;
}

/**
 * @brief 计算电机PID控制输出
 *
 * @param motor 电机实例指针
 * @param ref 电机控制参考值
 */
void DJIMotorSetPIDRef(DJIMotorInstance* motor, float ref) {
  // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
  motor->motor_controller.pid_ref = ref;
  Motor_Control_Setting_s* motor_setting;  // 电机控制参数
  Motor_Controller_s* motor_controller;    // 电机控制器
  DJI_Motor_Measure_s* measure;            // 电机测量值
  float pid_measure, pid_ref;              // 电机PID测量值和设定值

  motor_setting = &motor->motor_settings;
  motor_controller = &motor->motor_controller;
  measure = &motor->measure;
  pid_ref = motor_controller->pid_ref;  // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
  if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) pid_ref *= -1;  // 设置反转

  // pid_ref会顺次通过被启用的闭环充当数据的载体
  // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
  if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) {
    if (motor_setting->angle_feedback_source == OTHER_FEED)
      pid_measure = *motor_controller->other_angle_feedback_ptr;
    else
      pid_measure = measure->total_angle;  // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
    // 更新pid_ref进入下一个环
    pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
  }

  // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
  if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) {
    if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD) pid_ref += *motor_controller->speed_feedforward_ptr;

    if (motor_setting->speed_feedback_source == OTHER_FEED)
      pid_measure = *motor_controller->other_speed_feedback_ptr;
    else  // MOTOR_FEED
      pid_measure = measure->speed_aps;
    // 更新pid_ref进入下一个环
    pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
  }

  // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
  if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD) pid_ref += *motor_controller->current_feedforward_ptr;
  if (motor_setting->close_loop_type & CURRENT_LOOP) {
    pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
  }

  // 获取最终输出
  motor->motor_controller.final_output = (int16_t)pid_ref;
}

/**
 * @brief 通过CAN总线发送电机控制命令
 *        遍历所有已注册的电机，将控制输出打包到对应的CAN发送缓冲区中，
 *        然后按组发送CAN帧
 */
void DJIMotorTask() {
  // 遍历flag,检查是否要发送这一帧报文
  // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
  uint8_t group, num;  // 电机组号和组内编号
  DJIMotorInstance* motor;
  int16_t set;

  // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
  for (size_t i = 0; i < idx; ++i) {
    // 减小访存开销,先保存指针引用
    motor = dji_motor_instance[i];

    // 将最终输出分组填入发送数据
    group = motor->sender_group;
    num = motor->message_num;
    set = (int16_t)(motor->motor_controller.final_output);

    sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8);          // 低八位
    sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff);  // 高八位

    // 若该电机处于停止状态,直接将buff置零
    if (motor->stop_flag == MOTOR_STOP) memset(sender_assignment[group].tx_buff + 2 * num, 0, sizeof(uint16_t));
  }
  for (size_t i = 0; i < 9; ++i) {
    if (sender_enable_flag[i]) {
      CANTransmit(&sender_assignment[i], 1);
    }
  }
}